/******************************************************************
编程环境：----------
        软件:Arduino2.3.4
        硬件:Mega2560主控板,Mega2560扩展版,陀螺仪*1，云台*1
            方口USB数据线*1, Windows11操作系统电脑, 杜邦线若干
        库函数:<Arduino.h>, <Wire.h>
              <Adafruit_MPU6050.h>, <Adafruit_Sensor.h>

实验目的：-----------
        将陀螺仪原始数据转换为滚转、俯仰角
        计算得到的滚转、俯仰角转换为云台动作

操作流程：----------
        详细请看操作视频.

实验现象：-----------
        ____________________________________________________________
        程序烧录后，打开电池仓开关。尝试手动转动陀螺仪模块，观察云台模块
        是否跟随陀螺仪转动而转动。详情请看视频.

实验接线简略图：----------
    ￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣
    电池仓         mega2560扩展版          陀螺仪模块     云台
    12V母座~~~~~~~~~板载供电口1                
                      5V --------------------5V
                      SCL--------------------SDA
                      SDA--------------------SCL
                      GND--------------------GND
                      PG5(4)=============================云台上面的舵机引脚
                      PH3(6)=============================云台下面的转台舵机
    ￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣
2024.12.19 Uni Tongchuang Intelligent Robot Technology CO.,Ltd
QQ交流群：811348489 （UNI机器人开源交流群）
*******************************************************************/
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include "uni_servo.h"
#include <Wire.h>

#define rate 115200
#define servo_num 2

Adafruit_MPU6050 mpu;
UNI_SERVO_S servo(4, 6); //声明4,6引脚舵机
float servel_parts = 100.0;        //设定舵机舵机份数
int value_init_int[servo_num] = {11, 100};//舵机初始角度int类型
float value_init_float[servo_num] = {11.0, 100.0};//舵机初始角度-float类型
float roll = 0.0;  //用于存储陀螺仪滚转角
float yaw  = 0.0;  //用于存储陀螺仪偏航角
float pitch = 0.0; //用于存储陀螺仪俯仰角

void setup() {
  delay(1500);//程序开机等待片刻
  Serial.begin(rate);//打开串口
  servo_init();//舵机参数初始化
  gyroscope_init();//初始化MPU6050
}

void loop() {
  //陀螺仪角度映射到云台动作
  gimbal_action();
}
